home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 4.iso / src / demos / GL / flight / mat.c < prev    next >
C/C++ Source or Header  |  1994-08-01  |  6KB  |  260 lines
  1. /*
  2.  * Copyright 1990, 1991, 1992, 1993, 1994, Silicon Graphics, Inc.
  3.  * All Rights Reserved.
  4.  *
  5.  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
  6.  * the contents of this file may not be disclosed to third parties, copied or
  7.  * duplicated in any form, in whole or in part, without the prior written
  8.  * permission of Silicon Graphics, Inc.
  9.  *
  10.  * RESTRICTED RIGHTS LEGEND:
  11.  * Use, duplication or disclosure by the Government is subject to restrictions
  12.  * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
  13.  * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
  14.  * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
  15.  * rights reserved under the Copyright Laws of the United States.
  16.  */
  17.  
  18. /*
  19.  *  flight/mat.c $Revision: 1.8 $
  20.  *
  21.  *  matrix and vector math routines
  22.  */
  23.  
  24. #include "flight.h"
  25.  
  26.  
  27. void identify_matrix(Matrix mat)
  28. {
  29.     mat[0][0] = 1.0;
  30.     mat[0][1] = 0.0;
  31.     mat[0][2] = 0.0;
  32.     mat[0][3] = 0.0;
  33.     mat[1][0] = 0.0;
  34.     mat[1][1] = 1.0;
  35.     mat[1][2] = 0.0;
  36.     mat[1][3] = 0.0;
  37.     mat[2][0] = 0.0;
  38.     mat[2][1] = 0.0;
  39.     mat[2][2] = 1.0;
  40.     mat[2][3] = 0.0;
  41.     mat[3][0] = 0.0;
  42.     mat[3][1] = 0.0;
  43.     mat[3][2] = 0.0;
  44.     mat[3][3] = 1.0;
  45. }
  46.  
  47.  
  48. /*
  49.  *  Multiply mat1 * mat2 and return result in mat2
  50.  */
  51. void matrix_multiply(Matrix mat1, Matrix mat2)
  52. {
  53.     int i, j;
  54.     float temp[4][4];
  55.  
  56.     for(i=0; i < 4; i++)
  57.     for(j=0; j < 4; j++)
  58.         temp[j][i] = mat1[j][0]*mat2[0][i] + mat1[j][1]*mat2[1][i] +
  59.              mat1[j][2]*mat2[2][i] + mat1[j][3]*mat2[3][i];
  60.     for(i=0; i < 4; i++)
  61.     for(j=0; j < 4; j++)
  62.         mat2[j][i] = temp[j][i];
  63. }
  64.  
  65.  
  66. /*
  67.  *  Multiply mat2 * mat1 and return result in mat2
  68.  */
  69. void matrix_post_multiply(Matrix mat1, Matrix mat2)
  70. {
  71.     int i, j;
  72.     float temp[4][4];
  73.  
  74.     for(i=0; i < 4; i++)
  75.     for(j=0; j < 4; j++)
  76.         temp[j][i] = mat2[j][0]*mat1[0][i] + mat2[j][1]*mat1[1][i] +
  77.              mat2[j][2]*mat1[2][i] + mat2[j][3]*mat1[3][i];
  78.     for(i=0; i < 4; i++)
  79.     for(j=0; j < 4; j++)
  80.         mat2[j][i] = temp[j][i];
  81. }
  82.  
  83.  
  84. /*
  85.  *  Premultiply mat by rotation matrix. Return result in mat.
  86.  */
  87. void matrix_rotate(Matrix mat, int angle, char axis)
  88. {
  89.     float s, c;
  90.     float tmp;
  91.  
  92.     gl_sincos(angle, &s, &c);
  93.  
  94.     if (axis == 'x' || axis == 'X')
  95.     {
  96.     tmp = c*mat[1][0] + s*mat[2][0];
  97.     mat[2][0] = -s*mat[1][0] + c*mat[2][0];
  98.     mat[1][0] = tmp;
  99.     tmp = c*mat[1][1] + s*mat[2][1];
  100.     mat[2][1] = -s*mat[1][1] + c*mat[2][1];
  101.     mat[1][1] = tmp;
  102.     tmp = c*mat[1][2] + s*mat[2][2];
  103.     mat[2][2] = -s*mat[1][2] + c*mat[2][2];
  104.     mat[1][2] = tmp;
  105.     tmp = c*mat[1][3] + s*mat[2][3];
  106.     mat[2][3] = -s*mat[1][3] + c*mat[2][3];
  107.     mat[1][3] = tmp;
  108.     }
  109.     else if (axis == 'y' || axis == 'Y')
  110.     {
  111.     tmp = c*mat[0][0] + -s*mat[2][0];
  112.     mat[2][0] = s*mat[0][0] + c*mat[2][0];
  113.     mat[0][0] = tmp;
  114.     tmp = c*mat[0][1] + -s*mat[2][1];
  115.     mat[2][1] = s*mat[0][1] + c*mat[2][1];
  116.     mat[0][1] = tmp;
  117.     tmp = c*mat[0][2] + -s*mat[2][2];
  118.     mat[2][2] = s*mat[0][2] + c*mat[2][2];
  119.     mat[0][2] = tmp;
  120.     tmp = c*mat[0][3] + -s*mat[2][3];
  121.     mat[2][3] = s*mat[0][3] + c*mat[2][3];
  122.     mat[0][3] = tmp;
  123.     }
  124.     else if (axis == 'z' || axis == 'Z')
  125.     {
  126.     tmp = c*mat[0][0] + s*mat[1][0];
  127.     mat[1][0] = -s*mat[0][0] + c*mat[1][0];
  128.     mat[0][0] = tmp;
  129.     tmp = c*mat[0][1] + s*mat[1][1];
  130.     mat[1][1] = -s*mat[0][1] + c*mat[1][1];
  131.     mat[0][1] = tmp;
  132.     tmp = c*mat[0][2] + s*mat[1][2];
  133.     mat[1][2] = -s*mat[0][2] + c*mat[1][2];
  134.     mat[0][2] = tmp;
  135.     tmp = c*mat[0][3] + s*mat[1][3];
  136.     mat[1][3] = -s*mat[0][3] + c*mat[1][3];
  137.     mat[0][3] = tmp;
  138.     }
  139. }
  140.  
  141.  
  142. /*
  143.  *  Postmultiply mat by rotation matrix. Return result in mat.
  144.  */
  145. void matrix_post_rotate(Matrix mat, int angle, char axis)
  146. {
  147.     float s, c;
  148.     float tmp;
  149.  
  150.     gl_sincos(angle, &s, &c);
  151.  
  152.     if (axis == 'x' || axis == 'X')
  153.     {
  154.     tmp = c*mat[0][1] + -s*mat[0][2];
  155.     mat[0][2] = s*mat[0][1] + c*mat[0][2];
  156.     mat[0][1] = tmp;
  157.     tmp = c*mat[1][1] + -s*mat[1][2];
  158.     mat[1][2] = s*mat[1][1] + c*mat[1][2];
  159.     mat[1][1] = tmp;
  160.     tmp = c*mat[2][1] + -s*mat[2][2];
  161.     mat[2][2] = s*mat[2][1] + c*mat[2][2];
  162.     mat[2][1] = tmp;
  163.     tmp = c*mat[3][1] + -s*mat[3][2];
  164.     mat[3][2] = s*mat[3][1] + c*mat[3][2];
  165.     mat[3][1] = tmp;
  166.     }
  167.     else if (axis == 'y' || axis == 'Y')
  168.     {
  169.     tmp = c*mat[0][0] + s*mat[0][2];
  170.     mat[0][2] = -s*mat[0][0] + c*mat[0][2];
  171.     mat[0][0] = tmp;
  172.     tmp = c*mat[1][0] + s*mat[1][2];
  173.     mat[1][2] = -s*mat[1][0] + c*mat[1][2];
  174.     mat[1][0] = tmp;
  175.     tmp = c*mat[2][0] + s*mat[2][2];
  176.     mat[2][2] = -s*mat[2][0] + c*mat[2][2];
  177.     mat[2][0] = tmp;
  178.     tmp = c*mat[3][0] + s*mat[3][2];
  179.     mat[3][2] = -s*mat[3][0] + c*mat[3][2];
  180.     mat[3][0] = tmp;
  181.     }
  182.     else if (axis == 'z' || axis == 'Z')
  183.     {
  184.     tmp = c*mat[0][0] + -s*mat[0][1];
  185.     mat[0][1] = s*mat[0][0] + c*mat[0][1];
  186.     mat[0][0] = tmp;
  187.     tmp = c*mat[1][0] + -s*mat[1][1];
  188.     mat[1][1] = s*mat[1][0] + c*mat[1][1];
  189.     mat[1][0] = tmp;
  190.     tmp = c*mat[2][0] + -s*mat[2][1];
  191.     mat[2][1] = s*mat[2][0] + c*mat[2][1];
  192.     mat[2][0] = tmp;
  193.     tmp = c*mat[3][0] + -s*mat[3][1];
  194.     mat[3][1] = s*mat[3][0] + c*mat[3][1];
  195.     mat[3][0] = tmp;
  196.     }
  197. }
  198.  
  199.  
  200. /*
  201.  *  Premultiply mat by translation matrix.  Return result in mat.
  202.  */
  203. void matrix_translate(Matrix mat, float x, float y, float z)
  204. {
  205.     mat[3][0] = x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0];
  206.     mat[3][1] = x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1];
  207.     mat[3][2] = x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2];
  208.     mat[3][3] = x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3];
  209. }
  210.  
  211.  
  212. /*
  213.  *  multiply vector iv by a translation matrix mat.  Return result in ov.
  214.  */
  215. void mult_vec(Matrix mat, float iv[4], float ov[4])
  216. {
  217.     int i;
  218.  
  219.     for(i=0; i < 4; i++)
  220.     ov[i] = iv[0]*mat[0][i] + iv[1]*mat[1][i] +
  221.         iv[2]*mat[2][i] + iv[3]*mat[3][i];
  222. }
  223.  
  224.  
  225. /*
  226.  *  print a matrix
  227.  */
  228. void matrix_print(Matrix mat)
  229. {
  230.     printf("mat = {{%f, %f, %f, %f}\n",
  231.        mat[0][0], mat[0][1], mat[0][2], mat[0][3]);
  232.     printf("       {%f, %f, %f, %f}\n",
  233.        mat[1][0], mat[1][1], mat[1][2], mat[1][3]);
  234.     printf("       {%f, %f, %f, %f}\n",
  235.        mat[2][0], mat[2][1], mat[2][2], mat[2][3]);
  236.     printf("       {%f, %f, %f, %f}}\n",
  237.        mat[3][0], mat[3][1], mat[3][2], mat[3][3]);
  238. }
  239.  
  240.  
  241. /*
  242.  *  find the position 'ov' that is 'len' away from 'iv0' in the direction of
  243.  *  'iv1'.
  244.  */
  245. void vec_len(float *iv0, float *iv1, float len, float *ov)
  246. {
  247.     float x, y, z, delta;
  248.  
  249.     x = iv0[0] - iv1[0];
  250.     y = iv0[1] - iv1[1];
  251.     z = iv0[2] - iv1[2];
  252.  
  253.     delta = len / fsqrt(x*x + y*y + z*z);
  254.  
  255.     ov[0] = iv0[0] - x * delta;
  256.     ov[1] = iv0[1] - y * delta;
  257.     ov[2] = iv0[2] - z * delta;
  258. }
  259.  
  260.